#!/usr/bin/python
import sys
import rospy
# import thread
from std_msgs.msg import String
import json
from adafruit_pn532.uart import PN532_UART
from adafruit_pn532.adafruit_pn532 import MIFARE_CMD_AUTH_B
import time
import serial
import serial.tools.list_ports
from pi_driver.srv import GetString, GetStringResponse,SetString, SetStringResponse

key = b"\xFF\xFF\xFF\xFF\xFF\xFF"

class RfidNode:
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing......" % (self.node_name))
        self.active = True
        self.busy = False
        self.initModule()
        self.srv_read_nfc_id = rospy.Service(
            "~read_nfc_id", GetString, self.cbReadNfcId)
        self.srv_read_block_data = rospy.Service(
            "~read_block_data", GetString, self.cbReadBlockData)
        self.srv_write_block_data = rospy.Service(
            "~write_block_data", SetString, self.cbWriteBlockData)   
        self.srv_write_block_bytes = rospy.Service(
            "~write_block_bytes", SetString, self.cbWriteBlockBytes)   
        rospy.loginfo("[%s] Initialized......" % (self.node_name))

    def initModule(self):
        time.sleep(0.5)
        serial_ports = [i[0] for i in serial.tools.list_ports.comports()]
        print(serial_ports)
        port = None
        for p in serial_ports:
            if 'USB' in p:
                port = p
        if port is None:
            return
            rospy.signal_shutdown('no rfid module detected, exit')
        else:
            try:
                self.uart = serial.Serial(port=port, baudrate=115200, bytesize=8, parity=serial.PARITY_NONE,
                                                stopbits=serial.STOPBITS_ONE, timeout=0.1)
                self.pn532 = PN532_UART(self.uart, debug=False)
                # Configure PN532 to communicate with MiFare cards
                self.pn532.SAM_configuration()
                ic, ver, rev, support = self.pn532.firmware_version
                print("Found PN532 with firmware version: {0}.{1}".format(ver, rev))
            except Exception as e:
                print(e)

    def chkBusy(self):
        while self.busy:
            time.sleep(0.5)

    def cbReadNfcId(self, msg):
        self.chkBusy()
        self.busy = True
        while self.active:
            try:
                uid = self.pn532.read_passive_target(timeout=0.5)
                if uid is not None:
                    self.pn532.power_down()
                    self.busy = False
                    return GetStringResponse(uid.hex())
            except Exception as e:
                print(e)
                self.initModule()
        return GetStringResponse()

    def cbReadBlockData(self, msg):
        param = json.loads(msg.data)
        block_num = param['block_num']
        self.chkBusy()
        self.busy = True
        while self.active:
            try:
                uid = self.pn532.read_passive_target(timeout=0.5)
                if uid is not None:
                    authenticated = self.pn532.mifare_classic_authenticate_block(uid, block_num, MIFARE_CMD_AUTH_B, key)
                    if not authenticated:
                        self.pn532.power_down()
                        self.busy = False
                        print("Authentication failed!")
                        return GetStringResponse(json.dumps({"error":'认证失败，不支持该卡片',"data":''}))
                    else:
                        block = self.pn532.mifare_classic_read_block(block_num)
                        self.pn532.power_down()
                        self.busy = False
                        print(block)
                        if block is not None:
                            return GetStringResponse(json.dumps({"error":'',"data":block.hex()}))
                        else:
                            break
            except Exception as e:
                print(e)
                self.initModule()
        return GetStringResponse(json.dumps({'error':'','data':''}))

    def cbWriteBlockData(self, msg):
        param = json.loads(msg.data)
        block_num = param['block_num']
        if block_num % 4 == 3:
            return SetStringResponse('无法写入该区块')
        data = param['data'].encode('utf-8')
        self.chkBusy()
        self.busy = True
        while self.active:
            try:
                uid = self.pn532.read_passive_target(timeout=0.5)
                if uid is not None:
                    authenticated = self.pn532.mifare_classic_authenticate_block(uid, block_num, MIFARE_CMD_AUTH_B, key)
                    if not authenticated:
                        self.pn532.power_down()
                        self.busy = False
                        print("Authentication failed!")
                        return SetStringResponse('认证失败，不支持该卡片')
                    else:
                        block = bytearray(16)
                        if len(data) <= 16:
                            block[:len(data)] = data
                        else:
                            block[0:16] = data[:16]
                        self.pn532.mifare_classic_write_block(block_num,block)
                        self.pn532.power_down()
                        self.busy = False
                        print(block)
                        return SetStringResponse('写入成功')
            except Exception as e:
                print(e)
                self.initModule()
        return SetStringResponse()

    def cbWriteBlockBytes(self, msg):
        param = json.loads(msg.data)
        block_num = param['block_num']
        try:
            data = bytes.fromhex(param['bytes'].replace('0x',''))
        except Exception as e:
            print(e)
            return SetStringResponse('输入数据有误')
        self.chkBusy()
        self.busy = True
        while self.active:
            try:
                uid = self.pn532.read_passive_target(timeout=0.5)
                if uid is not None:
                    authenticated = self.pn532.mifare_classic_authenticate_block(uid, block_num, MIFARE_CMD_AUTH_B, key)
                    if not authenticated:
                        self.pn532.power_down()
                        self.busy = False
                        print("Authentication failed!")
                        return SetStringResponse('认证失败，不支持该卡片')
                    else:
                        block = bytearray(16)
                        if len(data) <= 16:
                            block[:len(data)] = data
                        else:
                            block[0:16] = data[:16]
                        self.pn532.mifare_classic_write_block(block_num,block)
                        self.pn532.power_down()
                        self.busy = False
                        print(block)
                        return SetStringResponse('写入成功')
            except Exception as e:
                print(e)
                self.initModule()
        return SetStringResponse()

    def onShutdown(self):
        self.active = False
        self.pn532.power_down()
        # self.pn532 = None
        self.uart.close()
        rospy.loginfo("[%s] shutdown......" % (self.node_name))


if __name__ == '__main__':
    rospy.init_node('rfid_node', anonymous=False)
    node = RfidNode()
    rospy.on_shutdown(node.onShutdown)
    # thread.start_new_thread(node.joy.start_open_loop, ())
    # node.joy.start_open_loop()
    rospy.spin()
